<html>
<head>
<meta http-equiv="Content-Type" content="text/html; charset=utf-8"/>
<script src="https://cdnjs.cloudflare.com/ajax/libs/echarts/4.2.1/echarts.min.js"></script>
<script src="./lib/echarts-gl.min.js"></script>

<script src="https://cdnjs.cloudflare.com/ajax/libs/mathjs/6.2.1/math.min.js"></script>

<script src="lib/Tangle-0.1.0/Tangle.js"></script>

<!-- TangleKit (optional) -->
<link rel="stylesheet" href="lib/Tangle-0.1.0/TangleKit/TangleKit.css" type="text/css">
<script type="text/javascript" src="lib/Tangle-0.1.0/TangleKit/mootools.js"></script>
<script type="text/javascript" src="lib/Tangle-0.1.0/TangleKit/sprintf.js"></script>
<script type="text/javascript" src="lib/Tangle-0.1.0/TangleKit/BVTouchable.js"></script>
<script type="text/javascript" src="lib/Tangle-0.1.0/TangleKit/TangleKit.js"></script>
<script>


// 判断相机坐标系下的某个点是否能被相机拍到
function pointCaptured(pw, ext, sx, sy, cols, rows, f) {
    var rotation = ext['rotation'], translation = ext['translation'];
    var pc = math.reshape(math.add(math.multiply(rotation, pw), translation), [3])
    var fx = f / sx;
    var fy = f / sy;
    var cx = cols / 2;
    var cy = rows / 2;
    var x = pc[0], y = pc[1], z = pc[2];
    var col = x * fx / z + cx;
    var row = y * fy / z + cy;
    return col >= 0 && col < cols && row >= 0 && row < rows;
}


// 判断是否所有点都被相机被到了
function pointsCaptured(points, ext, sx, sy, cols, rows, f) {
    for (var point of points) {
        if (!pointCaptured(math.reshape(point, [3, 1]), ext, sx, sy, cols, rows, f))
            return false;
    }
    return true;
}


function calcPoints(height, width, length) {
    return [
        [-width / 2, -length / 2, 0],
        [-width / 2, length / 2, 0],
        [width / 2, length / 2, 0],
        [width / 2, -length / 2, 0],
        [-width / 2, -length / 2, height],
        [-width / 2, length / 2, height],
        [width / 2, length / 2, height],
        [width / 2, -length / 2, height]
    ];
}


function normalize(vector) {
    return math.divide(vector, math.norm(vector));
}


// 求解双目相机的左右相机的外参数
function calcExtrinsic(cx, cy, cz, nx, ny, nz, d, b) {
    // z轴沿nx，ny，nz方向，x轴平行于地面，据此求出三个坐标轴
    var vz = normalize([nx, ny, nz]);
    var vx = normalize([-ny, nx, 0]);
    var vy = math.cross(vz, vx);

    // 相机光心的世界坐标
    var cameraOrigin = math.subtract([cx, cy, cz], math.multiply(normalize([nx, ny, nz]), d));

    // 世界坐标转相机坐标所需要的旋转矩阵和平移矩阵
    var rotationMatrix = math.reshape(math.concat(vx, vy, vz), [3, 3]);
    var translationVector = math.unaryMinus(math.multiply(rotationMatrix, cameraOrigin));

    return [
        {
        // 'origin': math.reshape(math.add(cameraOrigin, [-b / 2, 0, 0]), [3, 1]),
        'rotation': rotationMatrix,
        'translation': math.reshape(math.add(translationVector, [b / 2, 0, 0]), [3, 1])
        },
        {
        // 'origin': math.reshape(math.add(cameraOrigin, [b / 2, 0, 0]), [3, 1]),
        'rotation': rotationMatrix,
        'translation': math.reshape(math.add(translationVector, [-b / 2, 0, 0]), [3, 1])
        }
    ]
}


// /*
// * 求相机从指定角度拍摄时，双目相机的位姿
// * angle0: 相机与水平面的夹角
// * angle1: 相机与y=0面的夹角
// * height: 测量空间的高度
// * points: 测量空间的边角点
// */
// function cameraPose(angle0, angle1, height, b, sx, sy, cols, rows, f, points) {
//     // 相机方向
//     x = Math.cos(angle1) * Math.cos(angle0);
//     y = Math.sin(angle1) * Math.cos(angle0);
//     z = -Math.sin(angle0);

//     // 认为相机应该对准测量空间的中心点C
//     var cx = 0, cy = 0, cz = height / 2;

//     var d0 = 100; // 记录d的下界
//     // 不断减小d0直到无法拍摄所有点
//     for (var i = 0; i < 1000; i++) {
//         var extLR = calcExtrinsic(cx, cy, cz, x, y, z, d0, b)
//         if (!pointsCaptured(points, extLR[0], sx, sy, cols, rows, f)
//             || !pointsCaptured(points, extLR[1], sx, sy, cols, rows, f)) {
//             break;
//         }
//         d0 = d0 / 2;
//     }
//     var d1 = 200; // 记录d的上界
//     // 不断增大d1直到能拍摄所有点
//     for (var i = 0; i < 1000; i++) {
//         var extLR = calcExtrinsic(cx, cy, cz, x, y, z, d1, b)
//         if (pointsCaptured(points, extLR[0], sx, sy, cols, rows, f)
//             && pointsCaptured(points, extLR[1], sx, sy, cols, rows, f)) {
//             break;
//         }
//         d1 = d1 * 2;
//     }
//     // 用二分法逼近，找到临界点
//     var d = (d0 + d1) / 2;
//     for (var i = 0; i < 100; i++) {
//         var extLR = calcExtrinsic(cx, cy, cz, x, y, z, d, b)
//         if (pointsCaptured(points, extLR[0], sx, sy, cols, rows, f)
//             && pointsCaptured(points, extLR[1], sx, sy, cols, rows, f)) {
//             d1 = d;
//             d = (d1 + d0) / 2;
//         }
//         else {
//             d0 = d;
//             d = (d1 + d0) / 2;
//         }
//     }
//     return calcExtrinsic(
//         // 认为相机应该对准测量空间的中心
//         cx, cy, cz,
//         // 相机指向
//         x, y, z,
//         // 相机到测量空间中心点的距离
//         d, b)
// }

var extList;
var myChart;

var model = {
        initialize: function() {
            this.cols = 1280;
            this.rows = 1024;
            this.sx = 4.8;
            this.sy = 4.8;
            this.f = 5;
            this.b = 100;
            this.theta = 45;
            this.theta0 = 0;
            this.n = 4;
            this.d = 1000;

            this.width = this.height = this.length = 400;

            this.range = 2000;
        },
        update: function() {
            extList = [];
            var theta = this.theta / 180 * math.pi;
            var step = math.pi / this. n * 2;
            var points = calcPoints(this.height, this.width, this.length);
            var covered = true;
            for (var i = 0; i < this.n; i++) {
                var theta0 = this.theta0 / 180 * math.pi + i * step;
                var extLR = calcExtrinsic(0, 0, this.height / 2, 
                    Math.cos(theta0) * Math.cos(theta),
                    Math.sin(theta0) * Math.cos(theta),
                    -Math.sin(theta),
                    this.d, this.b
                );
                if (!pointsCaptured(points, extLR[0], this.sx / 1e3, this.sy / 1e3, this.cols, this.rows, this.f)
                    || !pointsCaptured(points, extLR[1], this.sx / 1e3, this.sy / 1e3, this.cols, this.rows, this.f))
                    covered = false;
                extList = extList.concat(extLR);
            }
            var ouputP = document.getElementById("output");
            ouputP.innerHTML = `
                <p style="color:${(covered ? "green" : "red")}">${(covered ? "能" : "不能") + "完全覆盖测量空间"}</p>
            `;
            this.plot(extList);
        },
        plot: function(extList) {
            myChart.clear();
            var length = this.length, width = this.width, height = this.height,
                f = this.f, sx = this.sx / 1e3, sy = this.sy / 1e3,
                cols = this.cols, rows = this.rows;
            var option = {
                tooltip: {},
                xAxis3D: {
                    type: 'value',
                },
                yAxis3D: {
                    type: 'value',
                },
                zAxis3D: {
                    type: 'value',
                    min: 0,
                },
                grid3D: {
                    viewControl: {
                        projection: 'projection'
                    }
                },
                series: []
            };
            
            function addLine3d(data, lineWidth) {
                lineWidth = lineWidth == null ? 1 : lineWidth;
                option.series.push({
                    type: 'line3D',
                    data: data,
                    lineStyle: {
                        width: lineWidth
                    }
                });
            }
            
            // 生成一个指定高度上的矩形
            function rect(x, y, z, w, h) {
                var hw = w / 2, hh = h / 2;
                var ret = [[x - hw, y - hh, z], [x - hw, y + hh, z], [x + hw, y + hh, z], [x + hw, y - hh, z], [x - hw, y - hh, z]];
                addLine3d(ret);
                return ret;
            }
            
            function line(x0, y0, z0, x1, y1, z1) {
                var ret = [[x0, y0, z0], [x1, y1, z1]];
                addLine3d(ret);
                return ret;
            }

            // 相机视场
            function plotCamera(ext) {
                function fieldLine(point1, origin) {
                        var x1 = point1[0], y1 = point1[1], z1 = point1[2];
                        var ox = origin[0], oy = origin[1], oz = origin[2];
                        var lambda = -oz / (oz - z1);
                        var ret =  [[x1, y1, z1], [ox + lambda * (ox - x1), oy + lambda * (oy - y1), oz + lambda * (oz - z1)]];
                        addLine3d(ret);
                        return ret;
                }
                var rotation = ext['rotation'];
                var translation = ext['translation']
                // 从相机坐标转到世界坐标所使用的外参
                var rotation2 = math.transpose(rotation);
                var translation2 = math.unaryMinus(math.multiply(rotation2, translation))
                var center = math.reshape(translation2, [3]);
                var halfW = cols * sx / 2, halfH = rows * sy / 2;
                for (var i = 0; i < 4; i++) {
                    var pc = math.reshape([(i < 2 ? -1 : 1) * halfW, (i ==1 || i == 2 ? -1 : 1) * halfH, -f], [3, 1]);
                    var pw = math.add(math.multiply(rotation2, pc), translation2);
                    fieldLine(math.reshape(pw, [3]), center);
                }
            }

            // 测量空间
            var x0 = width / 2, y0 = length / 2;
            var line1 = rect(0, 0, 0, width, length);
            var line2 = rect(0, 0, height, width, length);
            var line3 = line(x0, y0, 0, x0, y0, height);
            var line4 = line(x0, -y0, 0, x0, -y0, height);
            var line5 = line(-x0, y0, 0, -x0, y0, height);
            var line6 = line(-x0, -y0, 0, -x0, -y0, height);

            // 相机
            for (var ext of extList) {
                // var cameraOrigin = ext['origin'];
                // line(cameraOrigin[0][0], cameraOrigin[1][0], cameraOrigin[2][0], 0, 0, height / 2);
                plotCamera(ext);
            }

            option.xAxis3D.min = option.yAxis3D.min = - this.range / 2;
            option.xAxis3D.max = option.yAxis3D.max = this.range / 2;
            option.zAxis3D.max = this.range;
            myChart.setOption(option);
        }
    };

function init() {
    myChart = echarts.init(document.getElementById('div_3dplot'));
    var rootElement = document.getElementById("param_calculator");
    var tangle = new Tangle(rootElement, model);
}

</script>
</head>
<body onload="init();">
<div style="position: absolute; width: 100%"> 
    <div id="div_3dplot" style="width: 600px; height:600px; position: relative; margin:0 auto"></div>
</div>
<div id="param_calculator" style="position:absolute">
    相机参数：cols = <span class="TKAdjustableNumber" data-var="cols" data-min="2" data-max="10000"> px</span>; <br>
    rows = <span class="TKAdjustableNumber" data-var="rows" data-min="2" data-max="10000"> px</span>; <br>
    sx = <span class="TKAdjustableNumber" data-var="sx"> um</span>; <br>
    sy = <span class="TKAdjustableNumber" data-var="sy"> um</span>; <br>
    焦距 f = <span class="TKAdjustableNumber" data-var="f" data-min="0" data-max="100"> mm</span>; <br>
    双目间距 b = <span class="TKAdjustableNumber" data-var="b" data-min="0" data-max="999"> mm</span>; <br>
    倾角 θ = <span class="TKAdjustableNumber" data-var="theta" data-min="0" data-max="90"> °</span>; <br>
    起始角度 θ0 = <span class="TKAdjustableNumber" data-var="theta0" data-min="0" data-max="360"> °</span>; <br>
    相机个数 n = <span class="TKAdjustableNumber" data-var="n"></span>; <br>
    相机到测量空间中心点的距离 d = <span class="TKAdjustableNumber" data-var="d" data-min="1" data-max="10000"> mm</span>; <br>
    <br>
    测量空间：width = <span class="TKNumberField" data-var="width" data-min="0" data-max="2000"> mm</span>; <br>
    length = <span class="TKNumberField" data-var="length" data-min="0" data-max="2000"> mm</span>; <br>
    height = <span class="TKNumberField" data-var="height" data-min="0" data-max="2000"> mm</span>; <br>
    <br>
    作图范围: range = <span class="TKAdjustableNumber" data-var="range" data-min="10" data-max="10000"> mm</span>; <br>
    <br>
    计算结果：<p id="output"></p>
    
</div>
</body>
</html>